#include "Compass.h"
#include <avr/eeprom.h>
#include "iic.h"


#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif

//#define MAG_ORIENTATION(x,y,z) {raw_data[0]=x;raw_data[1]=z;raw_data[2]=y;}


compass_setting_t mag_setting_eeprom EEMEM;

static float safe_sqrt(float v)
{
    float ret = sqrt(v);
    if (isnan(ret))
    {
        return 0;
    }
    return ret;
}



void Compass::init()
{
    delay(100);
    i2c_writeReg(0X3C ,0x01 ,0x20 );
    i2c_writeReg(0X3C ,0x02 ,0x00 ); //register: Mode register  --  value: Continuous-Conversion Mode
    //t_cal = -1;
    t_cal = 0;
    //eeprom_read_block(zero, Compass_zero_eeprom, sizeof(zero));
    //vector.zero();
    heading = heading_y = 0;
    heading_x = 1;
    setting.init();
    //start_smooth = true;
    //zero[0]=zero[1]=zero[2] = 0;
}

void Compass::calibrate()
{
    t_cal = millis() + 30000;
    for(uint8_t axis = 0; axis < 3; axis++)
        sm.ext.min[axis] = sm.ext.max[axis] = setting.zero[axis] = data[axis] = 0;
}

void Compass::load_setting(void)
{
		if (eeprom_read_byte(&mag_setting_eeprom.mark) == 'C')
	  	 eeprom_read_block(&setting, &mag_setting_eeprom, sizeof(setting));
}

inline void Compass::save_setting()
{
    eeprom_write_block(&setting, &mag_setting_eeprom, sizeof(setting));
}

inline void Compass::read(int16_t out[])
{
    i2c_getSixRawADC(0X3C,0X03);

    out[0] = (rawADC[4]<<8) | rawADC[5];
    out[1] = -((rawADC[2]<<8) | rawADC[3]);
    out[2] = -((rawADC[0]<<8) | rawADC[1]);
}

void Compass::update()
{
    uint8_t	axis;
    int16_t raw[3];
    uint32_t now = millis();

    read(raw);

    if (t_cal > 0)
    {
        if (now < t_cal)   //����ִ��30SУ׼
        {
            for(axis=0; axis<3; axis++)
            {
                if (raw[axis] < sm.ext.min[axis]) sm.ext.min[axis] = raw[axis];
                if (raw[axis] > sm.ext.max[axis]) sm.ext.max[axis] = raw[axis];
                raw[axis] = 0;
            }
        }
        else
        {
            t_cal = 0;
            for(axis=0; axis<3; axis++)
                setting.zero[axis] = (sm.ext.min[axis] + sm.ext.max[axis]) >> 1;

            save_setting();
        }
    }

    for (axis = 0; axis < 3; axis++)
    {
        data[axis] = raw[axis] - setting.zero[axis];
        //vector[axis] = data[axis];
    }
}


void Compass::calculate(const Matrix3f& dcm_matrix)
{
    float headX;
    float headY;
    float cos_pitch = safe_sqrt(1-(dcm_matrix.c.x*dcm_matrix.c.x));

    // sin(pitch) = - dcm_matrix(3,1)
    // cos(pitch)*sin(roll) = - dcm_matrix(3,2)
    // cos(pitch)*cos(roll) = - dcm_matrix(3,3)

    if (cos_pitch == 0.0)
    {
        // we are pointing straight up or down so don't update our
        // heading using the compass. Wait for the next iteration when
        // we hopefully will have valid values again.
        return;
    }

    // Tilt compensated magnetic field X component:
    headX = data[0]*cos_pitch - data[1]*dcm_matrix.c.y*dcm_matrix.c.x/cos_pitch - data[2]*dcm_matrix.c.z*dcm_matrix.c.x/cos_pitch;
    // Tilt compensated magnetic field Y component:
    headY = data[1]*dcm_matrix.c.z/cos_pitch - data[2]*dcm_matrix.c.y/cos_pitch;

    // magnetic heading
    // 6/4/11 - added constrain to keep bad values from ruining DCM Yaw - Jason S.
    heading = constrain(atan2(-headY,headX), -3.15, 3.15);



    // Optimization for external DCM use. Calculate normalized components
    heading_x = cos(heading);
    heading_y = sin(heading);
}


void Compass::calculate(float roll, float pitch)
{
//  Note - This function implementation is deprecated
//  The alternate implementation of this function using the dcm matrix is preferred
    float headX;
    float headY;
    float cos_roll;
    float sin_roll;
    float cos_pitch;
    float sin_pitch;
    cos_roll = cos(roll);
    sin_roll = sin(roll);
    cos_pitch = cos(pitch);
    sin_pitch = sin(pitch);

    // Tilt compensated magnetic field X component:
    headX = data[0]*cos_pitch + data[1]*sin_roll*sin_pitch + data[2]*cos_roll*sin_pitch;
    // Tilt compensated magnetic field Y component:
    headY = data[1]*cos_roll - data[2]*sin_roll;
    // magnetic heading
    heading = atan2(-headY,headX);

    // Optimization for external DCM use. Calculate normalized components
    heading_x = cos(heading);
    heading_y = sin(heading);
}